#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <pantilt_control/PtuGotoAction.h>
#include <geometry_msgs/PointStamped.h>

int main (int argc, char **argv)
{
 ros::init(argc, argv, "test_ptu");
 actionlib::SimpleActionClient<pantilt_control::PtuGotoAction> ac("/SetPTUState",true);
 ROS_INFO("staaaaart!wait4server!");
 ac.waitForServer();
 ROS_INFO("CONNECTED");
 pantilt_control::PtuGotoGoal goal;


 goal.pan = atof(argv[1]);
 goal.tilt = atof(argv[2]);
 ac.sendGoal(goal);
 bool finished_before_timeout=ac.waitForResult();
 if (finished_before_timeout)
 {
     actionlib::SimpleClientGoalState state = ac.getState();

     ROS_INFO ("Action FInished %s", state.toString().c_str());
 }
 else
 ROS_INFO ("Action not Finished!");
 
	 return 0;
	 
}
